import numpy as np

#创建向量
x=np.array([1,2,3,5,8])
print(x)


#二维直角坐标系
#设定旋转角度，以30度为例
#将30度转化为弧度
theta=np.radians(30)
#创建旋转矩阵
Rotation_matrix=np.array([
    [np.cos(theta),-np.sin(theta)],
    [np.sin(theta),np.cos(theta)]
    ])

#假设我们有一个点
a=5
b=3
point=np.array([a,b])
#t通过旋转得到这个点的坐标
rotated_point=Rotation_matrix.dot(point)

print("原坐标为：",point)
print("旋转后的坐标为：",rotated_point)



#三维直角坐标系
#定义旋转角度，以弧度为单位
alpha=np.radians(30)#绕Z轴转动
beta=np.radians(45)#绕Y轴转动
gamma=np.radians(60)#绕X轴转动

#定义旋转矩阵
R_z=np.array([[np.cos(alpha),-np.sin(alpha),0],
              [np.sin(alpha),np.cos(alpha),0],
              [0,0,1]])
R_y=np.array([[np.cos(beta),0,np.sin(beta)],
              [0,1,0],
              [-np.sin(beta),0,np.cos(beta)]])
R_x=np.array([[1,0,0],
              [0,np.cos(gamma),-np.sin(gamma)],
              [0,np.sin(gamma),np.cos(gamma)]])

#总旋转矩阵
#@表示矩阵乘法
#按照Z轴、Y轴和X轴的顺序进行旋转，计算总的旋转矩阵
R=R_z @ R_y @ R_x
#定义点的坐标
P=np.array([1, 2, 3])
P_Rotated=R @ P

print("旋转后P点的坐标为:",P_Rotated)


#欧拉角度：按照Z-Y-X

# 定义欧拉角（以弧度为单位）
alpha1 = np.radians(30)  # 绕 z 轴的 Yaw 角度
beta1 = np.radians(45)   # 绕 y 轴的 Pitch 角度
gamma1 = np.radians(60)  # 绕 x 轴的 Roll 角度

# 构建对应的旋转矩阵
R_Z = np.array([[np.cos(alpha1), -np.sin(alpha1), 0],
                [np.sin(alpha1), np.cos(alpha1), 0],
                [0, 0, 1]])
R_Y = np.array([[np.cos(beta1), 0, np.sin(beta1)],
                [0, 1, 0],
                [-np.sin(beta1), 0, np.cos(beta1)]])
R_X = np.array([[1, 0, 0],
                [0, np.cos(gamma1), -np.sin(gamma1)],
                [0, np.sin(gamma1), np.cos(gamma1)]])

# 总旋转矩阵，注意乘法的顺序
R1 = np.dot(R_X, np.dot(R_Y, R_Z))
print("组合旋转矩阵为:")
print(R1)
# 组合旋转矩阵为:
# [[ 0.61237244 -0.35355339  0.70710678]
#  [ 0.78033009  0.12682648 -0.61237244]
#  [ 0.12682648  0.9267767   0.35355339]]



